#include "precomp.h"
#include "EntityMovement.h"
#include "../Entity.h"
#include "../../Map/Grid.h"

namespace Tmpl {

#define MOVEMENT_JOBS		16
#define MAX_MOVEMENT_ENT	64

static DataValuePoolBuffered<EntityMovement, EntityMovementPos> gMovements;
static Entity** gEntArrays[MOVEMENT_JOBS];

// EntitBaseyMovementystem setup functions
void EntityMovementSystem::Init()
{
	for (int i=0; i<MOVEMENT_JOBS; ++i)
	{
		gEntArrays[i] = MALLOC64(MAX_MOVEMENT_ENT, Entity*);
	}
	gMovements.Init();
}
void EntityMovementSystem::Clear()
{
	for (int i=0; i<MOVEMENT_JOBS; ++i)
	{
		FREE64(gEntArrays[i]);
	}
	gMovements.Clear();
}
int EntityMovementSystem::NewMovement(Entity* owner, Vec2f pos, Vec2f facing, const UnitData& data)
{
	int id = gMovements.NewValue();

	// Update Positional Data
	EntityMovementPos* last = gMovements.GetLast(id);
	EntityMovementPos* next = gMovements.GetNext(id);
	next->size = last->size = data.unit.size;
	next->speed = last->speed = data.unit.speed;
	next->pos = last->pos = pos;
	next->facing = last->facing = facing;
	
	// Update Parameters
	EntityMovement* value = gMovements.GetValue(id);
	value->mMoveId = id;
	value->mOwner = owner;
	value->mMaxSpeed = data.unit.speed;

	// add entity to grid
	Grid::AddEntity(value->mOwner, Grid::GetCellIdFromPos(next->pos));

	return id;
}
void EntityMovementSystem::RemoveMovement(int id)
{
	// remov entity from grid
	EntityMovement* value = gMovements.GetValue(id);
	EntityMovementPos* last = gMovements.GetLast(id);
	Grid::RemoveEntity(value->mOwner, Grid::GetCellIdFromPos(last->pos));

	gMovements.RemoveValue(id);
}
EntityMovement* EntityMovementSystem::GetMovement(int id)
{
	return gMovements.GetValue(id);
}
// EntityMovement - functions
void EntityMovement::SetSpeed(float speed)
{
	gMovements.GetNext(mMoveId)->speed = Min(speed, mMaxSpeed);
}
void EntityMovement::SetSize(float size)
{
	gMovements.GetNext(mMoveId)->size = size;
}
void EntityMovement::SetPos(Vec2f pos)
{
	gMovements.GetNext(mMoveId)->pos = pos;
}
void EntityMovement::SetFacing(Vec2f facing)
{
	gMovements.GetNext(mMoveId)->facing = facing;
}

float EntityMovement::GetSpeed() const
{
	return gMovements.GetLast(mMoveId)->speed;
}
float EntityMovement::GetSize() const
{
	return gMovements.GetLast(mMoveId)->size;
}
Vec2f EntityMovement::GetPos() const
{
	return gMovements.GetLast(mMoveId)->pos;
}
Vec2f EntityMovement::GetFacing() const
{
	return gMovements.GetLast(mMoveId)->facing;
}

float EntityMovement::GetNextSpeed() const
{
	return gMovements.GetNext(mMoveId)->speed;
}
float EntityMovement::GetNextSize() const
{
	return gMovements.GetNext(mMoveId)->size;
}
Vec2f EntityMovement::GetNextPos() const
{
	return gMovements.GetNext(mMoveId)->pos;
}
Vec2f EntityMovement::GetNextFacing() const
{
	return gMovements.GetNext(mMoveId)->facing;
}


// Updating
void EntityMovementSystem::UpdateMovementChunk(int chunk)
{
	int count = gMovements.GetValueCount();
	int step = count / MOVEMENT_JOBS;
	int from = step * chunk;
	int to = ((chunk==MOVEMENT_JOBS-1) ? count : step * (chunk+1));
	for (int i=from; i<to; ++i)
	{
		EntityMovement* v = gMovements.GetUnmappedValue(i);
		EntityMovementPos* last = gMovements.GetUnmappedLast(i);
		EntityMovementPos* next = gMovements.GetUnmappedNext(i);

		// update variables
		last->facing = next->facing;
		last->speed = next->speed;
		last->size = next->size;

		// update pos
		next->pos += next->facing * next->speed;
	}
}
void EntityMovementSystem::SolveOverlapChunk(int chunk)
{
	Entity** entArray = gEntArrays[chunk];
	int count = gMovements.GetValueCount();
	int step = count / MOVEMENT_JOBS;
	int from = step * chunk;
	int to = ((chunk==MOVEMENT_JOBS-1) ? count : step * (chunk+1));
	for (int i=from; i<to; ++i)
	{
		EntityMovement* v = gMovements.GetUnmappedValue(i);
		EntityMovementPos* last = gMovements.GetUnmappedLast(i);
		EntityMovementPos* next = gMovements.GetUnmappedNext(i);

		// seperation
		float radius = last->size + 0.05f;
		int entCount = Grid::SelectEntitiesInRangeFast(entArray, MAX_MOVEMENT_ENT, next->pos, radius + 0.5f, Entity::ENTITY_HERO | Entity::ENTITY_UNIT | Entity::ENTITY_DOODADD);
		for (int e=0; e<entCount; ++e)
		{
			Entity* ent = entArray[e];
			if (ent != v->mOwner)
			{
				// distance between the 2 units
				Vec2f entPos = ent->GetPos();
				Vec2f vDist = entPos - next->pos;
				float len = vDist.Length();
				if (len < 0.0001f)
					vDist = next->facing;
				else
					vDist /= len;
	
				// Seperation
				float dot = next->facing.Dot(vDist);
				float bRadius = (ent->GetSize() + radius);
				float overlap = bRadius - len;
				if (overlap > 0.0f)
				{
					next->pos -= vDist * Min(overlap, overlap * 0.1f + (overlap+1.0f) * overlap * 0.5f);
				}
				//// Flow System
				//else if (dot < 0.75f)
				//{
				//	float scalar = (len - bRadius + 1.0f);
				//	next->pos -= vDist * next->speed * 0.1f / scalar;
				//}

				// obstacle avoidance
				float r = ent->GetSize() + next->size + 0.01f;
				Vec2f localPos = PointToLocalSpace(entPos, next->facing, next->facing.Perp(), last->pos);
				if (localPos.x > 0.0f && fabs(localPos.y) < r)
				{
					Vec2f force = Vec2f(0.0f, (r - localPos.y) / localPos.x * 0.01f);
					next->pos += VectorToWorldSpace(force, next->facing, next->facing.Perp());
				}
			}
		}

		// solve collision with buildings
		Ray2f r = Ray2f(last->pos, next->pos);
		if (r.GetLength() > 0.0f)
		{
			Vec2f center;
			float intersect = -1.0f;
			int posX = int(next->pos.x);
			int posY = int(next->pos.y);
			for (int y=posY-1; y<=posY+1; ++y)
			for (int x=posX-1; x<=posX+1; ++x)
			{
				int collisionId = Grid::GetCollisionIdFromXY(x,y);
				switch (collisionId)
				{
				case Grid::COLLISION_NONE:
					break;
				default:
					AABB2f cell = Grid::GetCellAABB(x,y, radius);
					float distance = r.IntersectBox(cell);
					if (distance >= 0.0f && (distance < intersect || intersect<0.0f))
					{
						center = Grid::GetCellCenter(x, y);
						intersect = distance;
					}
				}
			}
			if (intersect >= 0.0f)
			{
				Vec2f vDir = (center - next->pos).Normalized(); 
				next->pos = r.GetPosAlongRay(intersect);
				next->pos -= vDir * 0.01f;
			}
		}

		// ensure max speed
		Vec2f vdist = next->pos - last->pos;
		if (vdist.SqrLength() > v->mMaxSpeed * v->mMaxSpeed)
		{
			vdist.Normalize();
			next->pos = last->pos + vdist * v->mMaxSpeed;
		}
	}
}
void EntityMovementSystem::UpdateGrid()
{
	int count = gMovements.GetValueCount();
	for (int i=0; i<count; ++i)
	{
		EntityMovement* v = gMovements.GetUnmappedValue(i);
		EntityMovementPos* last = gMovements.GetUnmappedLast(i);
		EntityMovementPos* next = gMovements.GetUnmappedNext(i);

		int lastCellId = Grid::GetCellIdFromPos(last->pos);
		int newCellId = Grid::GetCellIdFromPos(next->pos);
		if (lastCellId != newCellId) Grid::UpdateEntity(v->mOwner, lastCellId, newCellId);

		last->pos = next->pos;
	}
}
JobHandle EntityMovementSystem::Update(JobHandle dependency)
{
	// Solve Movement Updates
	JobHandle movementUpdate = JobManager::NewJob();
	for (int i=0; i<MOVEMENT_JOBS; ++i)
	{
		JobHandle job = JobManager::NewJob(&EntityMovementSystem::UpdateMovementChunk,i);
		JobManager::AddJobDependency(job, dependency);
		JobManager::AddExecuteJob(job);

		// add this job to the final job
		JobManager::AddJobDependency(movementUpdate, job);
	}
	JobManager::AddExecuteJob(movementUpdate);

	// Solve Overlap
	JobHandle overlapUpdate = JobManager::NewJob();
	for (int i=0; i<MOVEMENT_JOBS; ++i)
	{
		JobHandle job = JobManager::NewJob(&EntityMovementSystem::SolveOverlapChunk,i);
		JobManager::AddJobDependency(job, movementUpdate);
		JobManager::AddExecuteJob(job);

		// add this job to the final job
		JobManager::AddJobDependency(overlapUpdate, job);
	}
	JobManager::AddExecuteJob(overlapUpdate);

	// Grid Update
	JobHandle gridUpdate = JobManager::NewJob(EntityMovementSystem::UpdateGrid);
	JobManager::AddJobDependency(gridUpdate, overlapUpdate);
	JobManager::AddExecuteJob(gridUpdate);

	return gridUpdate;
}
}